%
% Path planning, generating field and maybe something about following it.
%

\section{Path planning}
To navigate the robot through the maze and find a good path to all the tags a path planning algorithm was needed. The path planning algorithm that was implemented for \emph{saint nr 9} bases on a map and a goal area, that is set as the area that should be reached (e.g. a tag area). The path finding algorithm just computes a solution for one goal area. To actually reach more or all tags an additional more global algorithm for path planning (e.g. TSP) would have been needed and was partly implemented in the \emph{Queen}.
\\

\subsection{Main idea}
\emph{Saint nr 9}'s path planning algorithm is inspired by the idea of potential fields. One problem with potential fields is that the robot could get stuck at a wall (e.g. for the given problem). In addition it is more difficult to handle different rotation angles than 90 degrees or to do smooth curve driving. 
One goal of the algorithm is therefore to avoid running into walls and to just allow 90 degree field changes.
Therefore the normal potential field force computation does not work in this case. 
\\

\subsection{Description of the algorithm}
A similar fitting algorithm is not known by the authors therefore verbal and real images are used to explain this algorithm.

The algorithm fills all known free space (using the configuration space) with a field. Unknown parts or walls are ignored an the field will be build around this areas.

We imagine that the field starts from the goal area in a given direction and flows in this direction until it hits a wall. If all field flows (if the area is larger there might be more than one pixel flow) hit a wall the direction is changed and the flow spreads from the existing computed forces into the other direction. 
\\

Cause we found it difficult to find words to describe the algorithm close, pictures are used to support the description of the flowing. 

In the first step is, as explained, the force in one given direction computed. The goal area is marked with \textbf{G} in figure \ref{pic:path_1}. As a start direction the the horizontal direction was chosen. The given start direction is useful to get the robot (in most cases) parallel to the wall/tag.  
\begin{figure}[!htb]
\centering
\includegraphics[width = 0.5\textwidth]{pic/path_planning_1}
\caption{First step of the path planning algorithm}
\label{pic:path_1}
\end{figure}

The shown arrows can not be more expanded. Therefore the direction of the flow is changed and the field flows now in vertical direction (as visualized in figure \ref{pic:path_2}.

\begin{figure}[!htb]
\centering
\includegraphics[width = 0.5\textwidth]{pic/path_planning_2}
\caption{Second step of the path planning algorithm, flow in vertical direction}
\label{pic:path_2}
\end{figure}

After this direction cannot expanded more the direction is changed again. Two more steps are visualized in figure \ref{pic:path_3} and \ref{pic:path_4}. This procedure is finished if the whole free space is filled with the field. \\
\begin{figure}
\centering
\begin{minipage}[hbt]{0.45\textwidth}
	\centering
	\includegraphics[width=\textwidth]{pic/path_planning_3}
	\caption{Third step, field flows again in vertical direction}
	\label{pic:path_3}
\end{minipage}
\hfill
\begin{minipage}[hbt]{0.45\textwidth}
	\centering
	\includegraphics[width=\textwidth]{pic/path_planning_4}
	\caption{Next step flowing in horizontal direction}
	\label{pic:path_4}
\end{minipage}
\end{figure}

\subsection{Implementation detail}
The implementation of this algorithm is close to a breadth search algorithm. The cells are queued in a priority queue. For the four directions the priority is decreased if the flow from the current cell to one child cell is in another direction (vertial vs. horizontal) than the direction of the parent cell to the current cell. Doing this, first all cells that flow in the at the moment interesting direction. All the others will be searched later.   

\subsection{Computation result}
The computed potential field map is based on the configuration space map that is build by the Map-Node after the first phase.
Due to the fact that the robot is not able to follow the field (Some message might not get where it should get) the potential field is visualized with arrows as shown in figure \ref{pic:potfield}.

\begin{figure}[!htb]
\centering
\includegraphics[width = 0.4\textwidth]{pic/potentialfield}
\caption{Output of the path planning}
\label{pic:potfield}
\end{figure}